﻿using System;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    [Serializable]
    public class BoxShape : IPhysicsShape
    {
        [HideInInspector]
        public Matrix4x4 matrix;
        public Vector3 offset;
        public Vector3 size;
        public bool forwardAxisMode;
        Vector3 mCacheExtents;

        public Bounds Bounds
        {
            get
            {
                var lossyScale = matrix.lossyScale;
                var sizeMag = new Vector3(size.x * lossyScale.x, size.y * lossyScale.y, size.z * lossyScale.z).magnitude;
                return new Bounds(GetWorldCenter(), Vector3.one * sizeMag);//考虑到旋转所以用模.
            }
        }

        public Vector3 P0 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, -mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P1 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, -mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P2 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P3 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, mCacheExtents.y, -mCacheExtents.z)); } }

        public Vector3 P4 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, -mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P5 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, -mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P6 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P7 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, mCacheExtents.y, mCacheExtents.z)); } }

        public Vector3 XAxis { get { return matrix.rotation * Vector3.right; } }
        public Vector3 YAxis { get { return matrix.rotation * Vector3.up; } }
        public Vector3 ZAxis { get { return matrix.rotation * Vector3.forward; } }


        public Vector3 GetWorldCenter()
        {
            return matrix.MultiplyPoint3x4(offset);
        }

        public bool IntersectsTest(IPhysicsShape other)
        {
            if (!Bounds.Intersects(other.Bounds)) return false;

            var sphereShape = other as SphereShape;
            if (sphereShape != null)
            {
                return PhysicsUtil.BoxVSSphere(matrix, offset, size, sphereShape.GetWorldCenter(), sphereShape.ConcertRadius);
            }

            var boxShape = other as BoxShape;
            if (boxShape != null)
            {
                mCacheExtents = size * 0.5f;
                boxShape.mCacheExtents = boxShape.size * 0.5f;

                return PhysicsUtil.BoxVSBox(P0, P1, P2, P3, P4, P5, P6, P7
                                                            , boxShape.P0, boxShape.P1, boxShape.P2, boxShape.P3, boxShape.P4, boxShape.P5, boxShape.P6, boxShape.P7
                                                            , XAxis, YAxis, ZAxis, boxShape.XAxis, boxShape.YAxis, boxShape.ZAxis);
            }

            var virtualCapsuleShape = other as VirtualCapsuleShape;
            if (virtualCapsuleShape != null)
            {
                return PhysicsUtil.BoxVSVirtualCapsule(matrix, offset, size, virtualCapsuleShape.matrix, virtualCapsuleShape.offset, virtualCapsuleShape.height, virtualCapsuleShape.radius, virtualCapsuleShape.direction, virtualCapsuleShape.virtualSegment);
            }

            return false;
        }

        public bool IntersectsTest(Vector3 lineBegin, Vector3 lineEnd, out Vector3 intersectPoint)
        {
            var result = false;
            intersectPoint = Vector3.zero;

            if (forwardAxisMode)
            {
                var planeNormal = matrix.rotation * Vector3.forward;
                mCacheExtents = size * 0.5f;
                var planePosition = matrix.MultiplyPoint3x4(offset + new Vector3(0, 0, mCacheExtents.z));
                var test_intersectPoint = PhysicsUtil.GetIntersectPoint(planeNormal, planePosition, lineBegin, lineEnd);
                if (test_intersectPoint != null)
                {
                    var temp = matrix.inverse.MultiplyPoint(test_intersectPoint.Value) - offset;
                    var sizeHalf = size * 0.5f;

                    if (temp.x <= sizeHalf.x
                        && temp.x >= -sizeHalf.x
                        && temp.y <= sizeHalf.y
                        && temp.y >= -sizeHalf.y)
                    {
                        intersectPoint = test_intersectPoint.Value;
                        result = true;
                    }
                }

                return result;
            }

            var direction = lineEnd - lineBegin;
            var distance = direction.magnitude;
            direction = direction.normalized;
            var ray = new Ray(lineBegin, direction);
            var intersectRayDistance = 0f;

            if (Bounds.IntersectRay(ray, out intersectRayDistance)
                && intersectRayDistance > distance)
            {
                return false;
            }

            var inverseMatrix = matrix.inverse;
            var local_LineBegin = inverseMatrix.MultiplyPoint3x4(lineBegin);
            var local_LineEnd = inverseMatrix.MultiplyPoint3x4(lineEnd);

            var local_direction = local_LineEnd - local_LineBegin;
            var local_distance = local_direction.magnitude;
            local_direction = local_direction.normalized;

            var localBounds = new Bounds(offset, size);
            var intersectDistance = 0f;
            result = localBounds.IntersectRay(new Ray(local_LineBegin, local_direction), out intersectDistance);

            if (intersectDistance > local_distance) result = false;

            if (result)
                intersectPoint = matrix.MultiplyPoint3x4(local_LineBegin + local_direction * intersectDistance);
            return result;
        }

        public bool IntersectsTest(Vector3 lineBegin, Vector3 lineEnd, out PhysicsHit physicsHit)
        {
            var result = false;
            physicsHit = default(PhysicsHit);

            if (forwardAxisMode)
            {
                var planeNormal = matrix.rotation * Vector3.forward;
                mCacheExtents = size * 0.5f;
                var planePosition = matrix.MultiplyPoint3x4(offset + new Vector3(0, 0, mCacheExtents.z));
                var test_intersectPoint = PhysicsUtil.GetIntersectPoint(planeNormal, planePosition, lineBegin, lineEnd);
                if (test_intersectPoint != null)
                {
                    var temp = matrix.inverse.MultiplyPoint(test_intersectPoint.Value) - offset;
                    var sizeHalf = size * 0.5f;

                    if (temp.x <= sizeHalf.x
                        && temp.x >= -sizeHalf.x
                        && temp.y <= sizeHalf.y
                        && temp.y >= -sizeHalf.y)
                    {
                        physicsHit.Point = test_intersectPoint.Value;
                        physicsHit.Normal = planeNormal;
                        physicsHit.PhysicsShape = this;
                        result = true;
                    }
                }

                return result;
            }

            var intersectPoint = Vector3.zero;
            var normal = Vector3.zero;
            var direction = lineBegin - lineEnd;
            var distance = direction.magnitude;
            direction = direction.normalized;
            var ray = new Ray(lineBegin, direction);
            var intersectRayDistance = 0f;
            if (Bounds.IntersectRay(ray, out intersectRayDistance)
                && intersectRayDistance > distance)
            {
                return false;
            }

            var inverseMatrix = matrix.inverse;
            var local_LineBegin = inverseMatrix.MultiplyPoint3x4(lineBegin);
            var local_LineEnd = inverseMatrix.MultiplyPoint3x4(lineEnd);

            var local_direction = local_LineEnd - local_LineBegin;
            var local_distance = local_direction.magnitude;
            local_direction = local_direction.normalized;

            var localBounds = new Bounds(offset, size);
            var intersectDistance = 0f;
            result = localBounds.IntersectRay(new Ray(local_LineBegin, local_direction), out intersectDistance);

            if (intersectDistance > local_distance) result = false;

            if (result)
            {
                intersectPoint = local_LineBegin + local_direction * intersectDistance;
                normal = GetNormal(intersectPoint, localBounds);

                physicsHit.Point = matrix.MultiplyPoint3x4(intersectPoint);
                physicsHit.Normal = matrix.rotation * normal;
                physicsHit.PhysicsShape = this;
            }

            return result;
        }

        public Vector3 IntersectsTest_GetNormal(Vector3 lineBegin, Vector3 lineEnd)
        {
            if (forwardAxisMode) return matrix.rotation * Vector3.forward;

            var result = Vector3.zero;
            var inverseMatrix = matrix.inverse;
            var localLineBegin = inverseMatrix.MultiplyPoint3x4(lineBegin);
            var localLineEnd = inverseMatrix.MultiplyPoint3x4(lineEnd);

            var localBounds = new Bounds(offset, size);

            var local_direction = localLineEnd - localLineBegin;
            local_direction = local_direction.normalized;

            var intersectDistance = 0f;
            var intersectPoint = Vector3.zero;
            var isIntersect = localBounds.IntersectRay(new Ray(localLineBegin, local_direction), out intersectDistance);

            if (isIntersect)
            {
                intersectPoint = localLineBegin + local_direction * intersectDistance;
                result = GetNormal(intersectPoint, localBounds);
            }
            else
            {
                throw new System.Exception("质点在Box外!");
            }

            return matrix.rotation * result;
        }

        public bool IsContainPoint(Vector3 point)
        {
            var sizeHalf = size * 0.5f;

            var compareMassPoint = matrix.inverse.MultiplyPoint3x4(point);

            if (compareMassPoint.x <= offset.x + sizeHalf.x && compareMassPoint.x >= offset.x - sizeHalf.x)
            {
                if (compareMassPoint.y <= offset.y + sizeHalf.y && compareMassPoint.y >= offset.y - sizeHalf.y)
                {
                    if (compareMassPoint.z <= offset.z + sizeHalf.z && compareMassPoint.z >= offset.z - sizeHalf.z)
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        public Vector3 MassPointToEdge(Vector3 massPoint, Vector3 outsidePoint)
        {
            var inverseMatrix = matrix.inverse;
            var local_MassPoint = inverseMatrix.MultiplyPoint3x4(massPoint);
            var local_OutsidePoint = inverseMatrix.MultiplyPoint3x4(outsidePoint);

            var local_direction = local_MassPoint - local_OutsidePoint;
            local_direction = local_direction.normalized;

            var localBounds = new Bounds(offset, size);
            var intersectDistance = 0f;
            var intersectPoint = Vector3.zero;
            var isIntersect = localBounds.IntersectRay(new Ray(local_OutsidePoint, local_direction), out intersectDistance);

            if (isIntersect)
                intersectPoint = matrix.MultiplyPoint3x4(local_OutsidePoint + local_direction * intersectDistance);
            else
                throw new System.Exception("质点在Box外!");

            return intersectPoint;
        }

        public void DrawGizmos(Color color)
        {
            var cacheMatrix = Gizmos.matrix;
            var cacheColor = Gizmos.color;

            Gizmos.matrix = matrix;

            Gizmos.color = color;
            Gizmos.DrawWireCube(offset, size);

            Gizmos.matrix = cacheMatrix;
            Gizmos.color = cacheColor;
        }

        Vector3 GetNormal(Vector3 intersectPoint, Bounds bounds)
        {
            var normal = Vector3.zero;
            var v1 = intersectPoint - bounds.min;
            var v2 = intersectPoint - bounds.max;

            const float NORMAL_EPS = 0.000001f;
            if (Mathf.Abs(v1.x) < NORMAL_EPS)
                normal = Vector3.left;
            else if (Mathf.Abs(v1.y) < NORMAL_EPS)
                normal = Vector3.down;
            else if (Mathf.Abs(v1.z) < NORMAL_EPS)
                normal = Vector3.back;
            else if (Mathf.Abs(v2.x) < NORMAL_EPS)
                normal = Vector3.right;
            else if (Mathf.Abs(v2.y) < NORMAL_EPS)
                normal = Vector3.up;
            else if (Mathf.Abs(v2.z) < NORMAL_EPS)
                normal = Vector3.forward;

            return normal;
        }

        Matrix4x4 IPhysicsShape.Matrix { get { return matrix; } set { matrix = value; } }
    }
}
